import win32api,win32con,time


def distance(t,a):
    temp = ((t[0]-a[0])**2+(t[1]-a[1])**2)**0.5
    print("距离：",temp)
    return temp

class PID:
    def __init__(self,kp,ki,kd,exp_v,acc_v,err,err_prev,err_last) -> None:
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.exp_v = exp_v
        self.acc_v = acc_v
        self.err = err
        self.err_prev = err_prev #e(k-2)
        self.err_last = err_last #e(k-1)
    def process(self,t,a):
        #err计算
        self.acc_v = distance(t,a)
        self.err = self.exp_v - self.acc_v
        add_v = self.kp*(self.err - self.err_last) + self.ki * self.err + self.kd*(self.err - 2*self.err_last + self.err_prev)
        self.err_prev = self.err_last
        self.err_last = self.err
        return add_v,self.acc_v